<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "https://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.9.1"/>
<meta name="viewport" content="width=device-width, initial-scale=1"/>
<title>Doxygen: registration/include/pcl/registration/impl/ndt_2d.hpp 源文件</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="navtree.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="resize.js"></script>
<script type="text/javascript" src="navtreedata.js"></script>
<script type="text/javascript" src="navtree.js"></script>
<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="search/searchdata.js"></script>
<script type="text/javascript" src="search/search.js"></script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectalign" style="padding-left: 0.5em;">
   <div id="projectname">Doxygen
   </div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- 制作者 Doxygen 1.9.1 -->
<script type="text/javascript">
/* @license magnet:?xt=urn:btih:cf05388f2679ee054f2beb29a391d25f4e673ac3&amp;dn=gpl-2.0.txt GPL-v2 */
var searchBox = new SearchBox("searchBox", "search",false,'搜索','.html');
/* @license-end */
</script>
<script type="text/javascript" src="menudata.js"></script>
<script type="text/javascript" src="menu.js"></script>
<script type="text/javascript">
/* @license magnet:?xt=urn:btih:cf05388f2679ee054f2beb29a391d25f4e673ac3&amp;dn=gpl-2.0.txt GPL-v2 */
$(function() {
  initMenu('',true,false,'search.php','搜索');
  $(document).ready(function() { init_search(); });
});
/* @license-end */</script>
<div id="main-nav"></div>
</div><!-- top -->
<div id="side-nav" class="ui-resizable side-nav-resizable">
  <div id="nav-tree">
    <div id="nav-tree-contents">
      <div id="nav-sync" class="sync"></div>
    </div>
  </div>
  <div id="splitbar" style="-moz-user-select:none;" 
       class="ui-resizable-handle">
  </div>
</div>
<script type="text/javascript">
/* @license magnet:?xt=urn:btih:cf05388f2679ee054f2beb29a391d25f4e673ac3&amp;dn=gpl-2.0.txt GPL-v2 */
$(document).ready(function(){initNavTree('ndt__2d_8hpp_source.html',''); initResizable(); });
/* @license-end */
</script>
<div id="doc-content">
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>

<div class="header">
  <div class="headertitle">
<div class="title">ndt_2d.hpp</div>  </div>
</div><!--header-->
<div class="contents">
<div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno">    1</span>&#160;<span class="comment">/*</span></div>
<div class="line"><a name="l00002"></a><span class="lineno">    2</span>&#160;<span class="comment"> * Software License Agreement (BSD License)</span></div>
<div class="line"><a name="l00003"></a><span class="lineno">    3</span>&#160;<span class="comment"> *</span></div>
<div class="line"><a name="l00004"></a><span class="lineno">    4</span>&#160;<span class="comment"> *  Point Cloud Library (PCL) - www.pointclouds.org</span></div>
<div class="line"><a name="l00005"></a><span class="lineno">    5</span>&#160;<span class="comment"> *  Copyright (c) 2011-2012, Willow Garage, Inc.</span></div>
<div class="line"><a name="l00006"></a><span class="lineno">    6</span>&#160;<span class="comment"> *  Copyright (c) 2012-, Open Perception, Inc.</span></div>
<div class="line"><a name="l00007"></a><span class="lineno">    7</span>&#160;<span class="comment"> *</span></div>
<div class="line"><a name="l00008"></a><span class="lineno">    8</span>&#160;<span class="comment"> *  All rights reserved.</span></div>
<div class="line"><a name="l00009"></a><span class="lineno">    9</span>&#160;<span class="comment"> *</span></div>
<div class="line"><a name="l00010"></a><span class="lineno">   10</span>&#160;<span class="comment"> *  Redistribution and use in source and binary forms, with or without</span></div>
<div class="line"><a name="l00011"></a><span class="lineno">   11</span>&#160;<span class="comment"> *  modification, are permitted provided that the following conditions</span></div>
<div class="line"><a name="l00012"></a><span class="lineno">   12</span>&#160;<span class="comment"> *  are met:</span></div>
<div class="line"><a name="l00013"></a><span class="lineno">   13</span>&#160;<span class="comment"> *</span></div>
<div class="line"><a name="l00014"></a><span class="lineno">   14</span>&#160;<span class="comment"> *   * Redistributions of source code must retain the above copyright</span></div>
<div class="line"><a name="l00015"></a><span class="lineno">   15</span>&#160;<span class="comment"> *     notice, this list of conditions and the following disclaimer.</span></div>
<div class="line"><a name="l00016"></a><span class="lineno">   16</span>&#160;<span class="comment"> *   * Redistributions in binary form must reproduce the above</span></div>
<div class="line"><a name="l00017"></a><span class="lineno">   17</span>&#160;<span class="comment"> *     copyright notice, this list of conditions and the following</span></div>
<div class="line"><a name="l00018"></a><span class="lineno">   18</span>&#160;<span class="comment"> *     disclaimer in the documentation and/or other materials provided</span></div>
<div class="line"><a name="l00019"></a><span class="lineno">   19</span>&#160;<span class="comment"> *     with the distribution.</span></div>
<div class="line"><a name="l00020"></a><span class="lineno">   20</span>&#160;<span class="comment"> *   * Neither the name of the copyright holder(s) nor the names of its</span></div>
<div class="line"><a name="l00021"></a><span class="lineno">   21</span>&#160;<span class="comment"> *     contributors may be used to endorse or promote products derived</span></div>
<div class="line"><a name="l00022"></a><span class="lineno">   22</span>&#160;<span class="comment"> *     from this software without specific prior written permission.</span></div>
<div class="line"><a name="l00023"></a><span class="lineno">   23</span>&#160;<span class="comment"> *</span></div>
<div class="line"><a name="l00024"></a><span class="lineno">   24</span>&#160;<span class="comment"> *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS</span></div>
<div class="line"><a name="l00025"></a><span class="lineno">   25</span>&#160;<span class="comment"> *  &quot;AS IS&quot; AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT</span></div>
<div class="line"><a name="l00026"></a><span class="lineno">   26</span>&#160;<span class="comment"> *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS</span></div>
<div class="line"><a name="l00027"></a><span class="lineno">   27</span>&#160;<span class="comment"> *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE</span></div>
<div class="line"><a name="l00028"></a><span class="lineno">   28</span>&#160;<span class="comment"> *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,</span></div>
<div class="line"><a name="l00029"></a><span class="lineno">   29</span>&#160;<span class="comment"> *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,</span></div>
<div class="line"><a name="l00030"></a><span class="lineno">   30</span>&#160;<span class="comment"> *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;</span></div>
<div class="line"><a name="l00031"></a><span class="lineno">   31</span>&#160;<span class="comment"> *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER</span></div>
<div class="line"><a name="l00032"></a><span class="lineno">   32</span>&#160;<span class="comment"> *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT</span></div>
<div class="line"><a name="l00033"></a><span class="lineno">   33</span>&#160;<span class="comment"> *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN</span></div>
<div class="line"><a name="l00034"></a><span class="lineno">   34</span>&#160;<span class="comment"> *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE</span></div>
<div class="line"><a name="l00035"></a><span class="lineno">   35</span>&#160;<span class="comment"> *  POSSIBILITY OF SUCH DAMAGE.</span></div>
<div class="line"><a name="l00036"></a><span class="lineno">   36</span>&#160;<span class="comment"> *</span></div>
<div class="line"><a name="l00037"></a><span class="lineno">   37</span>&#160;<span class="comment"> * $Id$</span></div>
<div class="line"><a name="l00038"></a><span class="lineno">   38</span>&#160;<span class="comment"> *</span></div>
<div class="line"><a name="l00039"></a><span class="lineno">   39</span>&#160;<span class="comment"> */</span></div>
<div class="line"><a name="l00040"></a><span class="lineno">   40</span>&#160;<span class="preprocessor">#ifndef PCL_NDT_2D_IMPL_H_</span></div>
<div class="line"><a name="l00041"></a><span class="lineno">   41</span>&#160;<span class="preprocessor">#define PCL_NDT_2D_IMPL_H_</span></div>
<div class="line"><a name="l00042"></a><span class="lineno">   42</span>&#160;<span class="preprocessor">#include &lt;cmath&gt;</span></div>
<div class="line"><a name="l00043"></a><span class="lineno">   43</span>&#160; </div>
<div class="line"><a name="l00044"></a><span class="lineno">   44</span>&#160;<span class="preprocessor">#include &lt;pcl/registration/eigen.h&gt;</span></div>
<div class="line"><a name="l00045"></a><span class="lineno">   45</span>&#160;<span class="preprocessor">#include &lt;pcl/registration/boost.h&gt;</span></div>
<div class="line"><a name="l00046"></a><span class="lineno">   46</span>&#160; </div>
<div class="line"><a name="l00047"></a><span class="lineno">   47</span>&#160;<span class="keyword">namespace </span>pcl</div>
<div class="line"><a name="l00048"></a><span class="lineno">   48</span>&#160;{</div>
<div class="line"><a name="l00049"></a><span class="lineno">   49</span>&#160;  <span class="keyword">namespace </span>ndt2d</div>
<div class="line"><a name="l00050"></a><span class="lineno">   50</span>&#160;  {</div>
<div class="line"><a name="l00055"></a><span class="lineno">   55</span>&#160;    <span class="keyword">template</span>&lt;<span class="keywordtype">unsigned</span> N=3, <span class="keyword">typename</span> T=<span class="keywordtype">double</span>&gt;</div>
<div class="line"><a name="l00056"></a><span class="lineno"><a class="line" href="structpcl_1_1ndt2d_1_1_value_and_derivatives.html">   56</a></span>&#160;    <span class="keyword">struct </span><a class="code" href="structpcl_1_1ndt2d_1_1_value_and_derivatives.html">ValueAndDerivatives</a></div>
<div class="line"><a name="l00057"></a><span class="lineno">   57</span>&#160;    {</div>
<div class="line"><a name="l00058"></a><span class="lineno">   58</span>&#160;      <a class="code" href="structpcl_1_1ndt2d_1_1_value_and_derivatives.html">ValueAndDerivatives</a> () : hessian (), grad (), value () {}</div>
<div class="line"><a name="l00059"></a><span class="lineno">   59</span>&#160; </div>
<div class="line"><a name="l00060"></a><span class="lineno">   60</span>&#160;      Eigen::Matrix&lt;T, N, N&gt; hessian;</div>
<div class="line"><a name="l00061"></a><span class="lineno">   61</span>&#160;      Eigen::Matrix&lt;T, N, 1&gt;    grad;</div>
<div class="line"><a name="l00062"></a><span class="lineno">   62</span>&#160;      T value;</div>
<div class="line"><a name="l00063"></a><span class="lineno">   63</span>&#160;      </div>
<div class="line"><a name="l00064"></a><span class="lineno">   64</span>&#160;      <span class="keyword">static</span> <a class="code" href="structpcl_1_1ndt2d_1_1_value_and_derivatives.html">ValueAndDerivatives&lt;N,T&gt;</a></div>
<div class="line"><a name="l00065"></a><span class="lineno">   65</span>&#160;      Zero ()</div>
<div class="line"><a name="l00066"></a><span class="lineno">   66</span>&#160;      {</div>
<div class="line"><a name="l00067"></a><span class="lineno">   67</span>&#160;        <a class="code" href="structpcl_1_1ndt2d_1_1_value_and_derivatives.html">ValueAndDerivatives&lt;N,T&gt;</a> r;</div>
<div class="line"><a name="l00068"></a><span class="lineno">   68</span>&#160;        r.hessian = Eigen::Matrix&lt;T, N, N&gt;::Zero ();</div>
<div class="line"><a name="l00069"></a><span class="lineno">   69</span>&#160;        r.grad    = Eigen::Matrix&lt;T, N, 1&gt;::Zero ();</div>
<div class="line"><a name="l00070"></a><span class="lineno">   70</span>&#160;        r.value   = 0;</div>
<div class="line"><a name="l00071"></a><span class="lineno">   71</span>&#160;        <span class="keywordflow">return</span> r;</div>
<div class="line"><a name="l00072"></a><span class="lineno">   72</span>&#160;      }</div>
<div class="line"><a name="l00073"></a><span class="lineno">   73</span>&#160; </div>
<div class="line"><a name="l00074"></a><span class="lineno">   74</span>&#160;      <a class="code" href="structpcl_1_1ndt2d_1_1_value_and_derivatives.html">ValueAndDerivatives&lt;N,T&gt;</a>&amp;</div>
<div class="line"><a name="l00075"></a><span class="lineno">   75</span>&#160;      operator+= (<a class="code" href="structpcl_1_1ndt2d_1_1_value_and_derivatives.html">ValueAndDerivatives&lt;N,T&gt;</a> <span class="keyword">const</span>&amp; r)</div>
<div class="line"><a name="l00076"></a><span class="lineno">   76</span>&#160;      {</div>
<div class="line"><a name="l00077"></a><span class="lineno">   77</span>&#160;        hessian += r.hessian;</div>
<div class="line"><a name="l00078"></a><span class="lineno">   78</span>&#160;        grad    += r.grad;</div>
<div class="line"><a name="l00079"></a><span class="lineno">   79</span>&#160;        value   += r.value;</div>
<div class="line"><a name="l00080"></a><span class="lineno">   80</span>&#160;        <span class="keywordflow">return</span> *<span class="keyword">this</span>;</div>
<div class="line"><a name="l00081"></a><span class="lineno">   81</span>&#160;      }</div>
<div class="line"><a name="l00082"></a><span class="lineno">   82</span>&#160;    };</div>
<div class="line"><a name="l00083"></a><span class="lineno">   83</span>&#160; </div>
<div class="line"><a name="l00095"></a><span class="lineno">   95</span>&#160;    <span class="keyword">template</span> &lt;<span class="keyword">typename</span> Po<span class="keywordtype">int</span>T&gt;</div>
<div class="line"><a name="l00096"></a><span class="lineno"><a class="line" href="classpcl_1_1ndt2d_1_1_normal_dist.html">   96</a></span>&#160;    <span class="keyword">class </span><a class="code" href="classpcl_1_1ndt2d_1_1_normal_dist.html">NormalDist</a></div>
<div class="line"><a name="l00097"></a><span class="lineno">   97</span>&#160;    {</div>
<div class="line"><a name="l00098"></a><span class="lineno">   98</span>&#160;      <span class="keyword">typedef</span> <a class="code" href="classpcl_1_1_point_cloud.html">pcl::PointCloud&lt;PointT&gt;</a> <a class="code" href="classpcl_1_1_point_cloud.html">PointCloud</a>;</div>
<div class="line"><a name="l00099"></a><span class="lineno">   99</span>&#160; </div>
<div class="line"><a name="l00100"></a><span class="lineno">  100</span>&#160;      <span class="keyword">public</span>:</div>
<div class="line"><a name="l00101"></a><span class="lineno">  101</span>&#160;        <a class="code" href="classpcl_1_1ndt2d_1_1_normal_dist.html">NormalDist</a> ()</div>
<div class="line"><a name="l00102"></a><span class="lineno">  102</span>&#160;          : min_n_ (3), n_ (0), pt_indices_ (), mean_ (), covar_inv_ ()</div>
<div class="line"><a name="l00103"></a><span class="lineno">  103</span>&#160;        {</div>
<div class="line"><a name="l00104"></a><span class="lineno">  104</span>&#160;        }</div>
<div class="line"><a name="l00105"></a><span class="lineno">  105</span>&#160;        </div>
<div class="line"><a name="l00109"></a><span class="lineno">  109</span>&#160;        <span class="keywordtype">void</span></div>
<div class="line"><a name="l00110"></a><span class="lineno"><a class="line" href="classpcl_1_1ndt2d_1_1_normal_dist.html#a35b40158d8a0c8884413d2219b3917ba">  110</a></span>&#160;        <a class="code" href="classpcl_1_1ndt2d_1_1_normal_dist.html#a35b40158d8a0c8884413d2219b3917ba">addIdx</a> (<span class="keywordtype">size_t</span> i)</div>
<div class="line"><a name="l00111"></a><span class="lineno">  111</span>&#160;        {</div>
<div class="line"><a name="l00112"></a><span class="lineno">  112</span>&#160;          pt_indices_.push_back (i);</div>
<div class="line"><a name="l00113"></a><span class="lineno">  113</span>&#160;        }</div>
<div class="line"><a name="l00114"></a><span class="lineno">  114</span>&#160;        </div>
<div class="line"><a name="l00119"></a><span class="lineno">  119</span>&#160;        <span class="keywordtype">void</span></div>
<div class="line"><a name="l00120"></a><span class="lineno"><a class="line" href="classpcl_1_1ndt2d_1_1_normal_dist.html#ad72ee95c77c17324d683c34915374cf1">  120</a></span>&#160;        <a class="code" href="classpcl_1_1ndt2d_1_1_normal_dist.html#ad72ee95c77c17324d683c34915374cf1">estimateParams</a> (<span class="keyword">const</span> <a class="code" href="classpcl_1_1_point_cloud.html">PointCloud</a>&amp; cloud, <span class="keywordtype">double</span> min_covar_eigvalue_mult = 0.001)</div>
<div class="line"><a name="l00121"></a><span class="lineno">  121</span>&#160;        {</div>
<div class="line"><a name="l00122"></a><span class="lineno">  122</span>&#160;          Eigen::Vector2d sx  = Eigen::Vector2d::Zero ();</div>
<div class="line"><a name="l00123"></a><span class="lineno">  123</span>&#160;          Eigen::Matrix2d sxx = Eigen::Matrix2d::Zero ();</div>
<div class="line"><a name="l00124"></a><span class="lineno">  124</span>&#160;          </div>
<div class="line"><a name="l00125"></a><span class="lineno">  125</span>&#160;          std::vector&lt;size_t&gt;::const_iterator i;</div>
<div class="line"><a name="l00126"></a><span class="lineno">  126</span>&#160;          <span class="keywordflow">for</span> (i = pt_indices_.begin (); i != pt_indices_.end (); i++)</div>
<div class="line"><a name="l00127"></a><span class="lineno">  127</span>&#160;          {</div>
<div class="line"><a name="l00128"></a><span class="lineno">  128</span>&#160;            Eigen::Vector2d p (cloud[*i]. x, cloud[*i]. y);</div>
<div class="line"><a name="l00129"></a><span class="lineno">  129</span>&#160;            sx  += p;</div>
<div class="line"><a name="l00130"></a><span class="lineno">  130</span>&#160;            sxx += p * p.transpose ();</div>
<div class="line"><a name="l00131"></a><span class="lineno">  131</span>&#160;          }</div>
<div class="line"><a name="l00132"></a><span class="lineno">  132</span>&#160;          </div>
<div class="line"><a name="l00133"></a><span class="lineno">  133</span>&#160;          n_ = pt_indices_.size ();</div>
<div class="line"><a name="l00134"></a><span class="lineno">  134</span>&#160; </div>
<div class="line"><a name="l00135"></a><span class="lineno">  135</span>&#160;          <span class="keywordflow">if</span> (n_ &gt;= min_n_)</div>
<div class="line"><a name="l00136"></a><span class="lineno">  136</span>&#160;          {</div>
<div class="line"><a name="l00137"></a><span class="lineno">  137</span>&#160;            mean_ = sx / <span class="keyword">static_cast&lt;</span><span class="keywordtype">double</span><span class="keyword">&gt;</span> (n_);</div>
<div class="line"><a name="l00138"></a><span class="lineno">  138</span>&#160;            <span class="comment">// Using maximum likelihood estimation as in the original paper</span></div>
<div class="line"><a name="l00139"></a><span class="lineno">  139</span>&#160;            Eigen::Matrix2d covar = (sxx - 2 * (sx * mean_.transpose ())) / <span class="keyword">static_cast&lt;</span><span class="keywordtype">double</span><span class="keyword">&gt;</span> (n_) + mean_ * mean_.transpose ();</div>
<div class="line"><a name="l00140"></a><span class="lineno">  140</span>&#160; </div>
<div class="line"><a name="l00141"></a><span class="lineno">  141</span>&#160;            Eigen::SelfAdjointEigenSolver&lt;Eigen::Matrix2d&gt; solver (covar);</div>
<div class="line"><a name="l00142"></a><span class="lineno">  142</span>&#160;            <span class="keywordflow">if</span> (solver.eigenvalues ()[0] &lt; min_covar_eigvalue_mult * solver.eigenvalues ()[1])</div>
<div class="line"><a name="l00143"></a><span class="lineno">  143</span>&#160;            {</div>
<div class="line"><a name="l00144"></a><span class="lineno">  144</span>&#160;              PCL_DEBUG (<span class="stringliteral">&quot;[pcl::NormalDist::estimateParams] NDT normal fit: adjusting eigenvalue %f\n&quot;</span>, solver.eigenvalues ()[0]);</div>
<div class="line"><a name="l00145"></a><span class="lineno">  145</span>&#160;              Eigen::Matrix2d l = solver.eigenvalues ().asDiagonal ();</div>
<div class="line"><a name="l00146"></a><span class="lineno">  146</span>&#160;              Eigen::Matrix2d q = solver.eigenvectors ();</div>
<div class="line"><a name="l00147"></a><span class="lineno">  147</span>&#160;              <span class="comment">// set minimum smallest eigenvalue:</span></div>
<div class="line"><a name="l00148"></a><span class="lineno">  148</span>&#160;              l (0,0) = l (1,1) * min_covar_eigvalue_mult;</div>
<div class="line"><a name="l00149"></a><span class="lineno">  149</span>&#160;              covar = q * l * q.transpose ();</div>
<div class="line"><a name="l00150"></a><span class="lineno">  150</span>&#160;            }</div>
<div class="line"><a name="l00151"></a><span class="lineno">  151</span>&#160;            covar_inv_ = covar.inverse ();</div>
<div class="line"><a name="l00152"></a><span class="lineno">  152</span>&#160;          }</div>
<div class="line"><a name="l00153"></a><span class="lineno">  153</span>&#160; </div>
<div class="line"><a name="l00154"></a><span class="lineno">  154</span>&#160;          pt_indices_.clear ();</div>
<div class="line"><a name="l00155"></a><span class="lineno">  155</span>&#160;        }</div>
<div class="line"><a name="l00156"></a><span class="lineno">  156</span>&#160; </div>
<div class="line"><a name="l00164"></a><span class="lineno">  164</span>&#160;        <a class="code" href="structpcl_1_1ndt2d_1_1_value_and_derivatives.html">ValueAndDerivatives&lt;3,double&gt;</a></div>
<div class="line"><a name="l00165"></a><span class="lineno"><a class="line" href="classpcl_1_1ndt2d_1_1_normal_dist.html#aebf7b0204e59622dbf5fce66edfb8f7c">  165</a></span>&#160;        <a class="code" href="classpcl_1_1ndt2d_1_1_normal_dist.html#aebf7b0204e59622dbf5fce66edfb8f7c">test</a> (<span class="keyword">const</span> <a class="code" href="structpcl_1_1_point_x_y_z_r_g_b_a.html">PointT</a>&amp; transformed_pt, <span class="keyword">const</span> <span class="keywordtype">double</span>&amp; cos_theta, <span class="keyword">const</span> <span class="keywordtype">double</span>&amp; sin_theta)<span class="keyword"> const</span></div>
<div class="line"><a name="l00166"></a><span class="lineno">  166</span>&#160;<span class="keyword">        </span>{</div>
<div class="line"><a name="l00167"></a><span class="lineno">  167</span>&#160;          <span class="keywordflow">if</span> (n_ &lt; min_n_)</div>
<div class="line"><a name="l00168"></a><span class="lineno">  168</span>&#160;            <span class="keywordflow">return</span> <a class="code" href="structpcl_1_1ndt2d_1_1_value_and_derivatives.html">ValueAndDerivatives&lt;3,double&gt;::Zero</a> ();</div>
<div class="line"><a name="l00169"></a><span class="lineno">  169</span>&#160;          </div>
<div class="line"><a name="l00170"></a><span class="lineno">  170</span>&#160;          <a class="code" href="structpcl_1_1ndt2d_1_1_value_and_derivatives.html">ValueAndDerivatives&lt;3,double&gt;</a> r;</div>
<div class="line"><a name="l00171"></a><span class="lineno">  171</span>&#160;          <span class="keyword">const</span> <span class="keywordtype">double</span> x = transformed_pt.x;</div>
<div class="line"><a name="l00172"></a><span class="lineno">  172</span>&#160;          <span class="keyword">const</span> <span class="keywordtype">double</span> y = transformed_pt.y;</div>
<div class="line"><a name="l00173"></a><span class="lineno">  173</span>&#160;          <span class="keyword">const</span> Eigen::Vector2d p_xy (transformed_pt.x, transformed_pt.y);</div>
<div class="line"><a name="l00174"></a><span class="lineno">  174</span>&#160;          <span class="keyword">const</span> Eigen::Vector2d q = p_xy - mean_;</div>
<div class="line"><a name="l00175"></a><span class="lineno">  175</span>&#160;          <span class="keyword">const</span> Eigen::RowVector2d qt_cvi (q.transpose () * covar_inv_);</div>
<div class="line"><a name="l00176"></a><span class="lineno">  176</span>&#160;          <span class="keyword">const</span> <span class="keywordtype">double</span> exp_qt_cvi_q = std::exp (-0.5 * <span class="keywordtype">double</span> (qt_cvi * q));</div>
<div class="line"><a name="l00177"></a><span class="lineno">  177</span>&#160;          r.value = -exp_qt_cvi_q;</div>
<div class="line"><a name="l00178"></a><span class="lineno">  178</span>&#160; </div>
<div class="line"><a name="l00179"></a><span class="lineno">  179</span>&#160;          Eigen::Matrix&lt;double, 2, 3&gt; jacobian;</div>
<div class="line"><a name="l00180"></a><span class="lineno">  180</span>&#160;          jacobian &lt;&lt;</div>
<div class="line"><a name="l00181"></a><span class="lineno">  181</span>&#160;            1, 0, -(x * sin_theta + y*cos_theta),</div>
<div class="line"><a name="l00182"></a><span class="lineno">  182</span>&#160;            0, 1,   x * cos_theta - y*sin_theta;</div>
<div class="line"><a name="l00183"></a><span class="lineno">  183</span>&#160;          </div>
<div class="line"><a name="l00184"></a><span class="lineno">  184</span>&#160;          <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; 3; i++)</div>
<div class="line"><a name="l00185"></a><span class="lineno">  185</span>&#160;            r.grad[i] = double (qt_cvi * jacobian.col (i)) * exp_qt_cvi_q;</div>
<div class="line"><a name="l00186"></a><span class="lineno">  186</span>&#160;          </div>
<div class="line"><a name="l00187"></a><span class="lineno">  187</span>&#160;          <span class="comment">// second derivative only for i == j == 2:</span></div>
<div class="line"><a name="l00188"></a><span class="lineno">  188</span>&#160;          <span class="keyword">const</span> Eigen::Vector2d d2q_didj (</div>
<div class="line"><a name="l00189"></a><span class="lineno">  189</span>&#160;              y * sin_theta - x*cos_theta,</div>
<div class="line"><a name="l00190"></a><span class="lineno">  190</span>&#160;            -(x * sin_theta + y*cos_theta)</div>
<div class="line"><a name="l00191"></a><span class="lineno">  191</span>&#160;          );</div>
<div class="line"><a name="l00192"></a><span class="lineno">  192</span>&#160; </div>
<div class="line"><a name="l00193"></a><span class="lineno">  193</span>&#160;          <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; 3; i++)</div>
<div class="line"><a name="l00194"></a><span class="lineno">  194</span>&#160;            <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> j = 0; j &lt; 3; j++)</div>
<div class="line"><a name="l00195"></a><span class="lineno">  195</span>&#160;              r.hessian (i,j) = -exp_qt_cvi_q * (</div>
<div class="line"><a name="l00196"></a><span class="lineno">  196</span>&#160;                double (-qt_cvi*jacobian.col (i)) * double (-qt_cvi*jacobian.col (j)) +</div>
<div class="line"><a name="l00197"></a><span class="lineno">  197</span>&#160;                (-qt_cvi * ((i==2 &amp;&amp; j==2)? d2q_didj : Eigen::Vector2d::Zero ())) +</div>
<div class="line"><a name="l00198"></a><span class="lineno">  198</span>&#160;                (-jacobian.col (j).transpose () * covar_inv_ * jacobian.col (i))</div>
<div class="line"><a name="l00199"></a><span class="lineno">  199</span>&#160;              );</div>
<div class="line"><a name="l00200"></a><span class="lineno">  200</span>&#160;          </div>
<div class="line"><a name="l00201"></a><span class="lineno">  201</span>&#160;          <span class="keywordflow">return</span> r;</div>
<div class="line"><a name="l00202"></a><span class="lineno">  202</span>&#160;        }</div>
<div class="line"><a name="l00203"></a><span class="lineno">  203</span>&#160; </div>
<div class="line"><a name="l00204"></a><span class="lineno">  204</span>&#160;    <span class="keyword">protected</span>:</div>
<div class="line"><a name="l00205"></a><span class="lineno">  205</span>&#160;        <span class="keyword">const</span> <span class="keywordtype">size_t</span> min_n_;</div>
<div class="line"><a name="l00206"></a><span class="lineno">  206</span>&#160; </div>
<div class="line"><a name="l00207"></a><span class="lineno">  207</span>&#160;        <span class="keywordtype">size_t</span> n_;</div>
<div class="line"><a name="l00208"></a><span class="lineno">  208</span>&#160;        std::vector&lt;size_t&gt; pt_indices_;</div>
<div class="line"><a name="l00209"></a><span class="lineno">  209</span>&#160;        Eigen::Vector2d mean_;</div>
<div class="line"><a name="l00210"></a><span class="lineno">  210</span>&#160;        Eigen::Matrix2d covar_inv_;</div>
<div class="line"><a name="l00211"></a><span class="lineno">  211</span>&#160;    };</div>
<div class="line"><a name="l00212"></a><span class="lineno">  212</span>&#160;    </div>
<div class="line"><a name="l00217"></a><span class="lineno">  217</span>&#160;    <span class="keyword">template</span> &lt;<span class="keyword">typename</span> Po<span class="keywordtype">int</span>T&gt; </div>
<div class="line"><a name="l00218"></a><span class="lineno"><a class="line" href="classpcl_1_1ndt2d_1_1_n_d_t_single_grid.html">  218</a></span>&#160;    <span class="keyword">class </span><a class="code" href="classpcl_1_1ndt2d_1_1_n_d_t_single_grid.html">NDTSingleGrid</a>: <span class="keyword">public</span> boost::noncopyable</div>
<div class="line"><a name="l00219"></a><span class="lineno">  219</span>&#160;    {</div>
<div class="line"><a name="l00220"></a><span class="lineno">  220</span>&#160;      <span class="keyword">typedef</span> <span class="keyword">typename</span> <a class="code" href="classpcl_1_1_point_cloud.html">pcl::PointCloud&lt;PointT&gt;</a> <a class="code" href="classpcl_1_1_point_cloud.html">PointCloud</a>;</div>
<div class="line"><a name="l00221"></a><span class="lineno">  221</span>&#160;      <span class="keyword">typedef</span> <span class="keyword">typename</span> pcl::PointCloud&lt;PointT&gt;::ConstPtr PointCloudConstPtr;</div>
<div class="line"><a name="l00222"></a><span class="lineno">  222</span>&#160;      <span class="keyword">typedef</span> <span class="keyword">typename</span> <a class="code" href="classpcl_1_1ndt2d_1_1_normal_dist.html">pcl::ndt2d::NormalDist&lt;PointT&gt;</a> <a class="code" href="classpcl_1_1ndt2d_1_1_normal_dist.html">NormalDist</a>;</div>
<div class="line"><a name="l00223"></a><span class="lineno">  223</span>&#160; </div>
<div class="line"><a name="l00224"></a><span class="lineno">  224</span>&#160;      <span class="keyword">public</span>:</div>
<div class="line"><a name="l00225"></a><span class="lineno">  225</span>&#160;        <a class="code" href="classpcl_1_1ndt2d_1_1_n_d_t_single_grid.html">NDTSingleGrid</a> (PointCloudConstPtr cloud,</div>
<div class="line"><a name="l00226"></a><span class="lineno">  226</span>&#160;                       <span class="keyword">const</span> Eigen::Vector2f&amp; about,</div>
<div class="line"><a name="l00227"></a><span class="lineno">  227</span>&#160;                       <span class="keyword">const</span> Eigen::Vector2f&amp; extent,</div>
<div class="line"><a name="l00228"></a><span class="lineno">  228</span>&#160;                       <span class="keyword">const</span> Eigen::Vector2f&amp; step)</div>
<div class="line"><a name="l00229"></a><span class="lineno">  229</span>&#160;            : min_ (about - extent), max_ (min_ + 2*extent), step_ (step),</div>
<div class="line"><a name="l00230"></a><span class="lineno">  230</span>&#160;              cells_ ((max_[0]-min_[0]) / step_[0],</div>
<div class="line"><a name="l00231"></a><span class="lineno">  231</span>&#160;                      (max_[1]-min_[1]) / step_[1]),</div>
<div class="line"><a name="l00232"></a><span class="lineno">  232</span>&#160;              normal_distributions_ (cells_[0], cells_[1])</div>
<div class="line"><a name="l00233"></a><span class="lineno">  233</span>&#160;        {</div>
<div class="line"><a name="l00234"></a><span class="lineno">  234</span>&#160;          <span class="comment">// sort through all points, assigning them to distributions:</span></div>
<div class="line"><a name="l00235"></a><span class="lineno">  235</span>&#160;          <a class="code" href="classpcl_1_1ndt2d_1_1_normal_dist.html">NormalDist</a>* n;</div>
<div class="line"><a name="l00236"></a><span class="lineno">  236</span>&#160;          <span class="keywordtype">size_t</span> used_points = 0;</div>
<div class="line"><a name="l00237"></a><span class="lineno">  237</span>&#160;          <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; cloud-&gt;size (); i++)</div>
<div class="line"><a name="l00238"></a><span class="lineno">  238</span>&#160;          <span class="keywordflow">if</span> ((n = <a class="code" href="classpcl_1_1ndt2d_1_1_n_d_t_single_grid.html#a097042ef93cf30db0cc0002ad80312b5">normalDistForPoint</a> (cloud-&gt;at (i))))</div>
<div class="line"><a name="l00239"></a><span class="lineno">  239</span>&#160;          {</div>
<div class="line"><a name="l00240"></a><span class="lineno">  240</span>&#160;            n-&gt;addIdx (i);</div>
<div class="line"><a name="l00241"></a><span class="lineno">  241</span>&#160;            used_points++;</div>
<div class="line"><a name="l00242"></a><span class="lineno">  242</span>&#160;          }</div>
<div class="line"><a name="l00243"></a><span class="lineno">  243</span>&#160; </div>
<div class="line"><a name="l00244"></a><span class="lineno">  244</span>&#160;          PCL_DEBUG (<span class="stringliteral">&quot;[pcl::NDTSingleGrid] NDT single grid %dx%d using %d/%d points\n&quot;</span>, cells_[0], cells_[1], used_points, cloud-&gt;size ());</div>
<div class="line"><a name="l00245"></a><span class="lineno">  245</span>&#160; </div>
<div class="line"><a name="l00246"></a><span class="lineno">  246</span>&#160;          <span class="comment">// then bake the distributions such that they approximate the</span></div>
<div class="line"><a name="l00247"></a><span class="lineno">  247</span>&#160;          <span class="comment">// points (and throw away memory of the points)</span></div>
<div class="line"><a name="l00248"></a><span class="lineno">  248</span>&#160;          <span class="keywordflow">for</span> (<span class="keywordtype">int</span> x = 0; x &lt; cells_[0]; x++)</div>
<div class="line"><a name="l00249"></a><span class="lineno">  249</span>&#160;            <span class="keywordflow">for</span> (<span class="keywordtype">int</span> y = 0; y &lt; cells_[1]; y++)</div>
<div class="line"><a name="l00250"></a><span class="lineno">  250</span>&#160;              normal_distributions_.coeffRef (x,y).estimateParams (*cloud);</div>
<div class="line"><a name="l00251"></a><span class="lineno">  251</span>&#160;        }</div>
<div class="line"><a name="l00252"></a><span class="lineno">  252</span>&#160;        </div>
<div class="line"><a name="l00258"></a><span class="lineno">  258</span>&#160;        <a class="code" href="structpcl_1_1ndt2d_1_1_value_and_derivatives.html">ValueAndDerivatives&lt;3,double&gt;</a></div>
<div class="line"><a name="l00259"></a><span class="lineno"><a class="line" href="classpcl_1_1ndt2d_1_1_n_d_t_single_grid.html#af77c36218ee572918dcef6435e6650a8">  259</a></span>&#160;        <a class="code" href="classpcl_1_1ndt2d_1_1_n_d_t_single_grid.html#af77c36218ee572918dcef6435e6650a8">test</a> (<span class="keyword">const</span> <a class="code" href="structpcl_1_1_point_x_y_z_r_g_b_a.html">PointT</a>&amp; transformed_pt, <span class="keyword">const</span> <span class="keywordtype">double</span>&amp; cos_theta, <span class="keyword">const</span> <span class="keywordtype">double</span>&amp; sin_theta)<span class="keyword"> const</span></div>
<div class="line"><a name="l00260"></a><span class="lineno">  260</span>&#160;<span class="keyword">        </span>{</div>
<div class="line"><a name="l00261"></a><span class="lineno">  261</span>&#160;          <span class="keyword">const</span> <a class="code" href="classpcl_1_1ndt2d_1_1_normal_dist.html">NormalDist</a>* n = <a class="code" href="classpcl_1_1ndt2d_1_1_n_d_t_single_grid.html#a097042ef93cf30db0cc0002ad80312b5">normalDistForPoint</a> (transformed_pt);</div>
<div class="line"><a name="l00262"></a><span class="lineno">  262</span>&#160;          <span class="comment">// index is in grid, return score from the normal distribution from</span></div>
<div class="line"><a name="l00263"></a><span class="lineno">  263</span>&#160;          <span class="comment">// the correct part of the grid:</span></div>
<div class="line"><a name="l00264"></a><span class="lineno">  264</span>&#160;          <span class="keywordflow">if</span> (n)</div>
<div class="line"><a name="l00265"></a><span class="lineno">  265</span>&#160;            <span class="keywordflow">return</span> n-&gt;test (transformed_pt, cos_theta, sin_theta);</div>
<div class="line"><a name="l00266"></a><span class="lineno">  266</span>&#160;          <span class="keywordflow">else</span></div>
<div class="line"><a name="l00267"></a><span class="lineno">  267</span>&#160;            <span class="keywordflow">return</span> <a class="code" href="structpcl_1_1ndt2d_1_1_value_and_derivatives.html">ValueAndDerivatives&lt;3,double&gt;::Zero</a> ();</div>
<div class="line"><a name="l00268"></a><span class="lineno">  268</span>&#160;        }</div>
<div class="line"><a name="l00269"></a><span class="lineno">  269</span>&#160; </div>
<div class="line"><a name="l00270"></a><span class="lineno">  270</span>&#160;      <span class="keyword">protected</span>:</div>
<div class="line"><a name="l00274"></a><span class="lineno">  274</span>&#160;        <a class="code" href="classpcl_1_1ndt2d_1_1_normal_dist.html">NormalDist</a>* </div>
<div class="line"><a name="l00275"></a><span class="lineno"><a class="line" href="classpcl_1_1ndt2d_1_1_n_d_t_single_grid.html#a097042ef93cf30db0cc0002ad80312b5">  275</a></span>&#160;        <a class="code" href="classpcl_1_1ndt2d_1_1_n_d_t_single_grid.html#a097042ef93cf30db0cc0002ad80312b5">normalDistForPoint</a> (<a class="code" href="structpcl_1_1_point_x_y_z_r_g_b_a.html">PointT</a> <span class="keyword">const</span>&amp; p)<span class="keyword"> const</span></div>
<div class="line"><a name="l00276"></a><span class="lineno">  276</span>&#160;<span class="keyword">        </span>{</div>
<div class="line"><a name="l00277"></a><span class="lineno">  277</span>&#160;          <span class="comment">// this would be neater in 3d...</span></div>
<div class="line"><a name="l00278"></a><span class="lineno">  278</span>&#160;          Eigen::Vector2f idxf;</div>
<div class="line"><a name="l00279"></a><span class="lineno">  279</span>&#160;          <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; 2; i++)</div>
<div class="line"><a name="l00280"></a><span class="lineno">  280</span>&#160;            idxf[i] = (p.getVector3fMap ()[i] - min_[i]) / step_[i];</div>
<div class="line"><a name="l00281"></a><span class="lineno">  281</span>&#160;          Eigen::Vector2i idxi = idxf.cast&lt;<span class="keywordtype">int</span>&gt; ();</div>
<div class="line"><a name="l00282"></a><span class="lineno">  282</span>&#160;          <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; 2; i++)</div>
<div class="line"><a name="l00283"></a><span class="lineno">  283</span>&#160;            <span class="keywordflow">if</span> (idxi[i] &gt;= cells_[i] || idxi[i] &lt; 0)</div>
<div class="line"><a name="l00284"></a><span class="lineno">  284</span>&#160;              <span class="keywordflow">return</span> NULL;</div>
<div class="line"><a name="l00285"></a><span class="lineno">  285</span>&#160;          <span class="comment">// const cast to avoid duplicating this function in const and</span></div>
<div class="line"><a name="l00286"></a><span class="lineno">  286</span>&#160;          <span class="comment">// non-const variants...</span></div>
<div class="line"><a name="l00287"></a><span class="lineno">  287</span>&#160;          <span class="keywordflow">return</span> <span class="keyword">const_cast&lt;</span><a class="code" href="classpcl_1_1ndt2d_1_1_normal_dist.html">NormalDist</a>*<span class="keyword">&gt;</span> (&amp;normal_distributions_.coeffRef (idxi[0], idxi[1]));</div>
<div class="line"><a name="l00288"></a><span class="lineno">  288</span>&#160;        }</div>
<div class="line"><a name="l00289"></a><span class="lineno">  289</span>&#160; </div>
<div class="line"><a name="l00290"></a><span class="lineno">  290</span>&#160;        Eigen::Vector2f min_;</div>
<div class="line"><a name="l00291"></a><span class="lineno">  291</span>&#160;        Eigen::Vector2f max_;</div>
<div class="line"><a name="l00292"></a><span class="lineno">  292</span>&#160;        Eigen::Vector2f step_;</div>
<div class="line"><a name="l00293"></a><span class="lineno">  293</span>&#160;        Eigen::Vector2i cells_;</div>
<div class="line"><a name="l00294"></a><span class="lineno">  294</span>&#160; </div>
<div class="line"><a name="l00295"></a><span class="lineno">  295</span>&#160;        Eigen::Matrix&lt;NormalDist, Eigen::Dynamic, Eigen::Dynamic&gt; normal_distributions_;</div>
<div class="line"><a name="l00296"></a><span class="lineno">  296</span>&#160;    };</div>
<div class="line"><a name="l00297"></a><span class="lineno">  297</span>&#160; </div>
<div class="line"><a name="l00304"></a><span class="lineno">  304</span>&#160;    <span class="keyword">template</span> &lt;<span class="keyword">typename</span> Po<span class="keywordtype">int</span>T&gt; </div>
<div class="line"><a name="l00305"></a><span class="lineno"><a class="line" href="classpcl_1_1ndt2d_1_1_n_d_t2_d.html">  305</a></span>&#160;    <span class="keyword">class </span><a class="code" href="classpcl_1_1ndt2d_1_1_n_d_t2_d.html">NDT2D</a>: <span class="keyword">public</span> boost::noncopyable</div>
<div class="line"><a name="l00306"></a><span class="lineno">  306</span>&#160;    {</div>
<div class="line"><a name="l00307"></a><span class="lineno">  307</span>&#160;      <span class="keyword">typedef</span> <span class="keyword">typename</span> <a class="code" href="classpcl_1_1_point_cloud.html">pcl::PointCloud&lt;PointT&gt;</a> <a class="code" href="classpcl_1_1_point_cloud.html">PointCloud</a>;</div>
<div class="line"><a name="l00308"></a><span class="lineno">  308</span>&#160;      <span class="keyword">typedef</span> <span class="keyword">typename</span> pcl::PointCloud&lt;PointT&gt;::ConstPtr PointCloudConstPtr;</div>
<div class="line"><a name="l00309"></a><span class="lineno">  309</span>&#160;      <span class="keyword">typedef</span> <a class="code" href="classpcl_1_1ndt2d_1_1_n_d_t_single_grid.html">NDTSingleGrid&lt;PointT&gt;</a> <a class="code" href="classpcl_1_1ndt2d_1_1_n_d_t_single_grid.html">SingleGrid</a>;</div>
<div class="line"><a name="l00310"></a><span class="lineno">  310</span>&#160; </div>
<div class="line"><a name="l00311"></a><span class="lineno">  311</span>&#160;      <span class="keyword">public</span>:</div>
<div class="line"><a name="l00318"></a><span class="lineno"><a class="line" href="classpcl_1_1ndt2d_1_1_n_d_t2_d.html#ac85a7448e491a3e7bbba99f482af9375">  318</a></span>&#160;        <a class="code" href="classpcl_1_1ndt2d_1_1_n_d_t2_d.html#ac85a7448e491a3e7bbba99f482af9375">NDT2D</a> (PointCloudConstPtr cloud,</div>
<div class="line"><a name="l00319"></a><span class="lineno">  319</span>&#160;             <span class="keyword">const</span> Eigen::Vector2f&amp; about,</div>
<div class="line"><a name="l00320"></a><span class="lineno">  320</span>&#160;             <span class="keyword">const</span> Eigen::Vector2f&amp; extent,</div>
<div class="line"><a name="l00321"></a><span class="lineno">  321</span>&#160;             <span class="keyword">const</span> Eigen::Vector2f&amp; step)</div>
<div class="line"><a name="l00322"></a><span class="lineno">  322</span>&#160;        {</div>
<div class="line"><a name="l00323"></a><span class="lineno">  323</span>&#160;          Eigen::Vector2f dx (step[0]/2, 0);</div>
<div class="line"><a name="l00324"></a><span class="lineno">  324</span>&#160;          Eigen::Vector2f dy (0, step[1]/2);</div>
<div class="line"><a name="l00325"></a><span class="lineno">  325</span>&#160;          single_grids_[0] = boost::make_shared&lt;SingleGrid&gt; (cloud, about,        extent, step);</div>
<div class="line"><a name="l00326"></a><span class="lineno">  326</span>&#160;          single_grids_[1] = boost::make_shared&lt;SingleGrid&gt; (cloud, about +dx,    extent, step);</div>
<div class="line"><a name="l00327"></a><span class="lineno">  327</span>&#160;          single_grids_[2] = boost::make_shared&lt;SingleGrid&gt; (cloud, about +dy,    extent, step);</div>
<div class="line"><a name="l00328"></a><span class="lineno">  328</span>&#160;          single_grids_[3] = boost::make_shared&lt;SingleGrid&gt; (cloud, about +dx+dy, extent, step);</div>
<div class="line"><a name="l00329"></a><span class="lineno">  329</span>&#160;        }</div>
<div class="line"><a name="l00330"></a><span class="lineno">  330</span>&#160;        </div>
<div class="line"><a name="l00336"></a><span class="lineno">  336</span>&#160;        <a class="code" href="structpcl_1_1ndt2d_1_1_value_and_derivatives.html">ValueAndDerivatives&lt;3,double&gt;</a></div>
<div class="line"><a name="l00337"></a><span class="lineno"><a class="line" href="classpcl_1_1ndt2d_1_1_n_d_t2_d.html#ad8bef04c21a6db1db8dec5f758e0a87b">  337</a></span>&#160;        <a class="code" href="classpcl_1_1ndt2d_1_1_n_d_t2_d.html#ad8bef04c21a6db1db8dec5f758e0a87b">test</a> (<span class="keyword">const</span> <a class="code" href="structpcl_1_1_point_x_y_z_r_g_b_a.html">PointT</a>&amp; transformed_pt, <span class="keyword">const</span> <span class="keywordtype">double</span>&amp; cos_theta, <span class="keyword">const</span> <span class="keywordtype">double</span>&amp; sin_theta)<span class="keyword"> const</span></div>
<div class="line"><a name="l00338"></a><span class="lineno">  338</span>&#160;<span class="keyword">        </span>{</div>
<div class="line"><a name="l00339"></a><span class="lineno">  339</span>&#160;          <a class="code" href="structpcl_1_1ndt2d_1_1_value_and_derivatives.html">ValueAndDerivatives&lt;3,double&gt;</a> r = <a class="code" href="structpcl_1_1ndt2d_1_1_value_and_derivatives.html">ValueAndDerivatives&lt;3,double&gt;::Zero</a> ();</div>
<div class="line"><a name="l00340"></a><span class="lineno">  340</span>&#160;          <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; 4; i++)</div>
<div class="line"><a name="l00341"></a><span class="lineno">  341</span>&#160;              r += single_grids_[i]-&gt;<a class="code" href="classpcl_1_1ndt2d_1_1_n_d_t2_d.html#ad8bef04c21a6db1db8dec5f758e0a87b">test</a> (transformed_pt, cos_theta, sin_theta);</div>
<div class="line"><a name="l00342"></a><span class="lineno">  342</span>&#160;          <span class="keywordflow">return</span> r;</div>
<div class="line"><a name="l00343"></a><span class="lineno">  343</span>&#160;        }</div>
<div class="line"><a name="l00344"></a><span class="lineno">  344</span>&#160; </div>
<div class="line"><a name="l00345"></a><span class="lineno">  345</span>&#160;      <span class="keyword">protected</span>:</div>
<div class="line"><a name="l00346"></a><span class="lineno">  346</span>&#160;        boost::shared_ptr&lt;SingleGrid&gt; single_grids_[4];</div>
<div class="line"><a name="l00347"></a><span class="lineno">  347</span>&#160;    };</div>
<div class="line"><a name="l00348"></a><span class="lineno">  348</span>&#160; </div>
<div class="line"><a name="l00349"></a><span class="lineno">  349</span>&#160;  } <span class="comment">// namespace ndt2d</span></div>
<div class="line"><a name="l00350"></a><span class="lineno">  350</span>&#160;} <span class="comment">// namespace pcl</span></div>
<div class="line"><a name="l00351"></a><span class="lineno">  351</span>&#160; </div>
<div class="line"><a name="l00352"></a><span class="lineno">  352</span>&#160; </div>
<div class="line"><a name="l00353"></a><span class="lineno">  353</span>&#160;<span class="keyword">namespace </span>Eigen</div>
<div class="line"><a name="l00354"></a><span class="lineno">  354</span>&#160;{</div>
<div class="line"><a name="l00355"></a><span class="lineno">  355</span>&#160;  <span class="comment">/* This NumTraits specialisation is necessary because NormalDist is used as</span></div>
<div class="line"><a name="l00356"></a><span class="lineno">  356</span>&#160;<span class="comment">   * the element type of an Eigen Matrix.</span></div>
<div class="line"><a name="l00357"></a><span class="lineno">  357</span>&#160;<span class="comment">   */</span></div>
<div class="line"><a name="l00358"></a><span class="lineno"><a class="line" href="struct_eigen_1_1_num_traits_3_01pcl_1_1ndt2d_1_1_normal_dist_3_01_point_t_01_4_01_4.html">  358</a></span>&#160;  <span class="keyword">template</span>&lt;<span class="keyword">typename</span> Po<span class="keywordtype">int</span>T&gt; <span class="keyword">struct </span>NumTraits&lt;pcl::ndt2d::NormalDist&lt;PointT&gt; &gt;</div>
<div class="line"><a name="l00359"></a><span class="lineno">  359</span>&#160;  {</div>
<div class="line"><a name="l00360"></a><span class="lineno">  360</span>&#160;    <span class="keyword">typedef</span> <span class="keywordtype">double</span> Real;</div>
<div class="line"><a name="l00361"></a><span class="lineno">  361</span>&#160;    <span class="keyword">typedef</span> <span class="keywordtype">double</span> Literal;</div>
<div class="line"><a name="l00362"></a><span class="lineno">  362</span>&#160;    <span class="keyword">static</span> Real dummy_precision () { <span class="keywordflow">return</span> 1.0; }</div>
<div class="line"><a name="l00363"></a><span class="lineno">  363</span>&#160;    <span class="keyword">enum</span> {</div>
<div class="line"><a name="l00364"></a><span class="lineno">  364</span>&#160;      IsComplex = 0,</div>
<div class="line"><a name="l00365"></a><span class="lineno">  365</span>&#160;      IsInteger = 0,</div>
<div class="line"><a name="l00366"></a><span class="lineno">  366</span>&#160;      IsSigned = 0,</div>
<div class="line"><a name="l00367"></a><span class="lineno">  367</span>&#160;      RequireInitialization = 1,</div>
<div class="line"><a name="l00368"></a><span class="lineno">  368</span>&#160;      ReadCost = 1,</div>
<div class="line"><a name="l00369"></a><span class="lineno">  369</span>&#160;      AddCost = 1,</div>
<div class="line"><a name="l00370"></a><span class="lineno">  370</span>&#160;      MulCost = 1</div>
<div class="line"><a name="l00371"></a><span class="lineno">  371</span>&#160;    };</div>
<div class="line"><a name="l00372"></a><span class="lineno">  372</span>&#160;  };</div>
<div class="line"><a name="l00373"></a><span class="lineno">  373</span>&#160;}</div>
<div class="line"><a name="l00374"></a><span class="lineno">  374</span>&#160; </div>
<div class="line"><a name="l00376"></a><span class="lineno">  376</span>&#160;<span class="keyword">template</span> &lt;<span class="keyword">typename</span> Po<span class="keywordtype">int</span>Source, <span class="keyword">typename</span> Po<span class="keywordtype">int</span>Target&gt; <span class="keywordtype">void</span></div>
<div class="line"><a name="l00377"></a><span class="lineno"><a class="line" href="classpcl_1_1_normal_distributions_transform2_d.html#abf99a15fe6808f4242da02091ec4b736">  377</a></span>&#160;<a class="code" href="classpcl_1_1_normal_distributions_transform2_d.html#abf99a15fe6808f4242da02091ec4b736">pcl::NormalDistributionsTransform2D&lt;PointSource, PointTarget&gt;::computeTransformation</a> (<a class="code" href="classpcl_1_1_point_cloud.html">PointCloudSource</a> &amp;output, <span class="keyword">const</span> Eigen::Matrix4f &amp;guess)</div>
<div class="line"><a name="l00378"></a><span class="lineno">  378</span>&#160;{</div>
<div class="line"><a name="l00379"></a><span class="lineno">  379</span>&#160;  <a class="code" href="classpcl_1_1_point_cloud.html">PointCloudSource</a> intm_cloud = output;</div>
<div class="line"><a name="l00380"></a><span class="lineno">  380</span>&#160; </div>
<div class="line"><a name="l00381"></a><span class="lineno">  381</span>&#160;  nr_iterations_ = 0;</div>
<div class="line"><a name="l00382"></a><span class="lineno">  382</span>&#160;  converged_ = <span class="keyword">false</span>;</div>
<div class="line"><a name="l00383"></a><span class="lineno">  383</span>&#160; </div>
<div class="line"><a name="l00384"></a><span class="lineno">  384</span>&#160;  <span class="keywordflow">if</span> (guess != Eigen::Matrix4f::Identity ())</div>
<div class="line"><a name="l00385"></a><span class="lineno">  385</span>&#160;  {</div>
<div class="line"><a name="l00386"></a><span class="lineno">  386</span>&#160;    transformation_ = guess;</div>
<div class="line"><a name="l00387"></a><span class="lineno">  387</span>&#160;    <a class="code" href="group__common.html#ga52d532f7f2b4d7bba78d13701d3a33d8">transformPointCloud</a> (output, intm_cloud, transformation_);</div>
<div class="line"><a name="l00388"></a><span class="lineno">  388</span>&#160;  } </div>
<div class="line"><a name="l00389"></a><span class="lineno">  389</span>&#160; </div>
<div class="line"><a name="l00390"></a><span class="lineno">  390</span>&#160;  <span class="comment">// build Normal Distribution Transform of target cloud:</span></div>
<div class="line"><a name="l00391"></a><span class="lineno">  391</span>&#160;  <a class="code" href="classpcl_1_1ndt2d_1_1_n_d_t2_d.html">ndt2d::NDT2D&lt;PointTarget&gt;</a> target_ndt (target_, grid_centre_, grid_extent_, grid_step_);</div>
<div class="line"><a name="l00392"></a><span class="lineno">  392</span>&#160;  </div>
<div class="line"><a name="l00393"></a><span class="lineno">  393</span>&#160;  <span class="comment">// can&#39;t seem to use .block&lt;&gt; () member function on transformation_</span></div>
<div class="line"><a name="l00394"></a><span class="lineno">  394</span>&#160;  <span class="comment">// directly... gcc bug? </span></div>
<div class="line"><a name="l00395"></a><span class="lineno">  395</span>&#160;  Eigen::Matrix4f&amp; transformation = transformation_;</div>
<div class="line"><a name="l00396"></a><span class="lineno">  396</span>&#160; </div>
<div class="line"><a name="l00397"></a><span class="lineno">  397</span>&#160; </div>
<div class="line"><a name="l00398"></a><span class="lineno">  398</span>&#160;  <span class="comment">// work with x translation, y translation and z rotation: extending to 3D</span></div>
<div class="line"><a name="l00399"></a><span class="lineno">  399</span>&#160;  <span class="comment">// would be some tricky maths, but not impossible.</span></div>
<div class="line"><a name="l00400"></a><span class="lineno">  400</span>&#160;  <span class="keyword">const</span> Eigen::Matrix3f initial_rot (transformation.block&lt;3,3&gt; (0,0));</div>
<div class="line"><a name="l00401"></a><span class="lineno">  401</span>&#160;  <span class="keyword">const</span> Eigen::Vector3f rot_x (initial_rot*Eigen::Vector3f::UnitX ());</div>
<div class="line"><a name="l00402"></a><span class="lineno">  402</span>&#160;  <span class="keyword">const</span> <span class="keywordtype">double</span> z_rotation = std::atan2 (rot_x[1], rot_x[0]);</div>
<div class="line"><a name="l00403"></a><span class="lineno">  403</span>&#160; </div>
<div class="line"><a name="l00404"></a><span class="lineno">  404</span>&#160;  Eigen::Vector3d xytheta_transformation (</div>
<div class="line"><a name="l00405"></a><span class="lineno">  405</span>&#160;    transformation (0,3),</div>
<div class="line"><a name="l00406"></a><span class="lineno">  406</span>&#160;    transformation (1,3),</div>
<div class="line"><a name="l00407"></a><span class="lineno">  407</span>&#160;    z_rotation</div>
<div class="line"><a name="l00408"></a><span class="lineno">  408</span>&#160;  );</div>
<div class="line"><a name="l00409"></a><span class="lineno">  409</span>&#160; </div>
<div class="line"><a name="l00410"></a><span class="lineno">  410</span>&#160;  <span class="keywordflow">while</span> (!converged_)</div>
<div class="line"><a name="l00411"></a><span class="lineno">  411</span>&#160;  {</div>
<div class="line"><a name="l00412"></a><span class="lineno">  412</span>&#160;    <span class="keyword">const</span> <span class="keywordtype">double</span> cos_theta = std::cos (xytheta_transformation[2]);</div>
<div class="line"><a name="l00413"></a><span class="lineno">  413</span>&#160;    <span class="keyword">const</span> <span class="keywordtype">double</span> sin_theta = std::sin (xytheta_transformation[2]);</div>
<div class="line"><a name="l00414"></a><span class="lineno">  414</span>&#160;    previous_transformation_ = transformation;    </div>
<div class="line"><a name="l00415"></a><span class="lineno">  415</span>&#160; </div>
<div class="line"><a name="l00416"></a><span class="lineno">  416</span>&#160;    <a class="code" href="structpcl_1_1ndt2d_1_1_value_and_derivatives.html">ndt2d::ValueAndDerivatives&lt;3, double&gt;</a> score = <a class="code" href="structpcl_1_1ndt2d_1_1_value_and_derivatives.html">ndt2d::ValueAndDerivatives&lt;3, double&gt;::Zero</a> ();</div>
<div class="line"><a name="l00417"></a><span class="lineno">  417</span>&#160;    <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i = 0; i &lt; intm_cloud.size (); i++)</div>
<div class="line"><a name="l00418"></a><span class="lineno">  418</span>&#160;      score += target_ndt.<a class="code" href="classpcl_1_1ndt2d_1_1_n_d_t2_d.html#ad8bef04c21a6db1db8dec5f758e0a87b">test</a> (intm_cloud[i], cos_theta, sin_theta);</div>
<div class="line"><a name="l00419"></a><span class="lineno">  419</span>&#160;    </div>
<div class="line"><a name="l00420"></a><span class="lineno">  420</span>&#160;    PCL_DEBUG (<span class="stringliteral">&quot;[pcl::NormalDistributionsTransform2D::computeTransformation] NDT score %f (x=%f,y=%f,r=%f)\n&quot;</span>,</div>
<div class="line"><a name="l00421"></a><span class="lineno">  421</span>&#160;      <span class="keywordtype">float</span> (score.value), xytheta_transformation[0], xytheta_transformation[1], xytheta_transformation[2]</div>
<div class="line"><a name="l00422"></a><span class="lineno">  422</span>&#160;    );</div>
<div class="line"><a name="l00423"></a><span class="lineno">  423</span>&#160; </div>
<div class="line"><a name="l00424"></a><span class="lineno">  424</span>&#160;    <span class="keywordflow">if</span> (score.value != 0)</div>
<div class="line"><a name="l00425"></a><span class="lineno">  425</span>&#160;    {</div>
<div class="line"><a name="l00426"></a><span class="lineno">  426</span>&#160;      <span class="comment">// test for positive definiteness, and adjust to ensure it if necessary:</span></div>
<div class="line"><a name="l00427"></a><span class="lineno">  427</span>&#160;      Eigen::EigenSolver&lt;Eigen::Matrix3d&gt; solver;</div>
<div class="line"><a name="l00428"></a><span class="lineno">  428</span>&#160;      solver.compute (score.hessian, <span class="keyword">false</span>);</div>
<div class="line"><a name="l00429"></a><span class="lineno">  429</span>&#160;      <span class="keywordtype">double</span> min_eigenvalue = 0;</div>
<div class="line"><a name="l00430"></a><span class="lineno">  430</span>&#160;      <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt;3; i++)</div>
<div class="line"><a name="l00431"></a><span class="lineno">  431</span>&#160;        <span class="keywordflow">if</span> (solver.eigenvalues ()[i].real () &lt; min_eigenvalue)</div>
<div class="line"><a name="l00432"></a><span class="lineno">  432</span>&#160;            min_eigenvalue = solver.eigenvalues ()[i].real ();</div>
<div class="line"><a name="l00433"></a><span class="lineno">  433</span>&#160; </div>
<div class="line"><a name="l00434"></a><span class="lineno">  434</span>&#160;      <span class="comment">// ensure &quot;safe&quot; positive definiteness: this is a detail missing</span></div>
<div class="line"><a name="l00435"></a><span class="lineno">  435</span>&#160;      <span class="comment">// from the original paper</span></div>
<div class="line"><a name="l00436"></a><span class="lineno">  436</span>&#160;      <span class="keywordflow">if</span> (min_eigenvalue &lt; 0)</div>
<div class="line"><a name="l00437"></a><span class="lineno">  437</span>&#160;      {</div>
<div class="line"><a name="l00438"></a><span class="lineno">  438</span>&#160;        <span class="keywordtype">double</span> lambda = 1.1 * min_eigenvalue - 1;</div>
<div class="line"><a name="l00439"></a><span class="lineno">  439</span>&#160;        score.hessian += Eigen::Vector3d (-lambda, -lambda, -lambda).asDiagonal ();</div>
<div class="line"><a name="l00440"></a><span class="lineno">  440</span>&#160;        solver.compute (score.hessian, <span class="keyword">false</span>);</div>
<div class="line"><a name="l00441"></a><span class="lineno">  441</span>&#160;        PCL_DEBUG (<span class="stringliteral">&quot;[pcl::NormalDistributionsTransform2D::computeTransformation] adjust hessian: %f: new eigenvalues:%f %f %f\n&quot;</span>,</div>
<div class="line"><a name="l00442"></a><span class="lineno">  442</span>&#160;            <span class="keywordtype">float</span> (lambda),</div>
<div class="line"><a name="l00443"></a><span class="lineno">  443</span>&#160;            solver.eigenvalues ()[0].real (),</div>
<div class="line"><a name="l00444"></a><span class="lineno">  444</span>&#160;            solver.eigenvalues ()[1].real (),</div>
<div class="line"><a name="l00445"></a><span class="lineno">  445</span>&#160;            solver.eigenvalues ()[2].real ()</div>
<div class="line"><a name="l00446"></a><span class="lineno">  446</span>&#160;        );</div>
<div class="line"><a name="l00447"></a><span class="lineno">  447</span>&#160;      }</div>
<div class="line"><a name="l00448"></a><span class="lineno">  448</span>&#160;      assert (solver.eigenvalues ()[0].real () &gt;= 0 &amp;&amp;</div>
<div class="line"><a name="l00449"></a><span class="lineno">  449</span>&#160;              solver.eigenvalues ()[1].real () &gt;= 0 &amp;&amp;</div>
<div class="line"><a name="l00450"></a><span class="lineno">  450</span>&#160;              solver.eigenvalues ()[2].real () &gt;= 0);</div>
<div class="line"><a name="l00451"></a><span class="lineno">  451</span>&#160;      </div>
<div class="line"><a name="l00452"></a><span class="lineno">  452</span>&#160;      Eigen::Vector3d delta_transformation (-score.hessian.inverse () * score.grad);</div>
<div class="line"><a name="l00453"></a><span class="lineno">  453</span>&#160;      Eigen::Vector3d new_transformation = xytheta_transformation + newton_lambda_.cwiseProduct (delta_transformation);</div>
<div class="line"><a name="l00454"></a><span class="lineno">  454</span>&#160; </div>
<div class="line"><a name="l00455"></a><span class="lineno">  455</span>&#160;      xytheta_transformation = new_transformation;</div>
<div class="line"><a name="l00456"></a><span class="lineno">  456</span>&#160;      </div>
<div class="line"><a name="l00457"></a><span class="lineno">  457</span>&#160;      <span class="comment">// update transformation matrix from x, y, theta:</span></div>
<div class="line"><a name="l00458"></a><span class="lineno">  458</span>&#160;      transformation.block&lt;3,3&gt; (0,0).matrix () = Eigen::Matrix3f (Eigen::AngleAxisf (<span class="keyword">static_cast&lt;</span><span class="keywordtype">float</span><span class="keyword">&gt;</span> (xytheta_transformation[2]), Eigen::Vector3f::UnitZ ()));</div>
<div class="line"><a name="l00459"></a><span class="lineno">  459</span>&#160;      transformation.block&lt;3,1&gt; (0,3).matrix () = Eigen::Vector3f (<span class="keyword">static_cast&lt;</span><span class="keywordtype">float</span><span class="keyword">&gt;</span> (xytheta_transformation[0]), <span class="keyword">static_cast&lt;</span><span class="keywordtype">float</span><span class="keyword">&gt;</span> (xytheta_transformation[1]), 0.0f);</div>
<div class="line"><a name="l00460"></a><span class="lineno">  460</span>&#160; </div>
<div class="line"><a name="l00461"></a><span class="lineno">  461</span>&#160;      <span class="comment">//std::cout &lt;&lt; &quot;new transformation:\n&quot; &lt;&lt; transformation &lt;&lt; std::endl;</span></div>
<div class="line"><a name="l00462"></a><span class="lineno">  462</span>&#160;    }</div>
<div class="line"><a name="l00463"></a><span class="lineno">  463</span>&#160;    <span class="keywordflow">else</span></div>
<div class="line"><a name="l00464"></a><span class="lineno">  464</span>&#160;    {</div>
<div class="line"><a name="l00465"></a><span class="lineno">  465</span>&#160;      PCL_ERROR (<span class="stringliteral">&quot;[pcl::NormalDistributionsTransform2D::computeTransformation] no overlap: try increasing the size or reducing the step of the grid\n&quot;</span>);</div>
<div class="line"><a name="l00466"></a><span class="lineno">  466</span>&#160;      <span class="keywordflow">break</span>;</div>
<div class="line"><a name="l00467"></a><span class="lineno">  467</span>&#160;    }</div>
<div class="line"><a name="l00468"></a><span class="lineno">  468</span>&#160;    </div>
<div class="line"><a name="l00469"></a><span class="lineno">  469</span>&#160;    <a class="code" href="group__common.html#ga52d532f7f2b4d7bba78d13701d3a33d8">transformPointCloud</a> (output, intm_cloud, transformation);</div>
<div class="line"><a name="l00470"></a><span class="lineno">  470</span>&#160; </div>
<div class="line"><a name="l00471"></a><span class="lineno">  471</span>&#160;    nr_iterations_++;</div>
<div class="line"><a name="l00472"></a><span class="lineno">  472</span>&#160;    </div>
<div class="line"><a name="l00473"></a><span class="lineno">  473</span>&#160;    <span class="keywordflow">if</span> (update_visualizer_ != 0)</div>
<div class="line"><a name="l00474"></a><span class="lineno">  474</span>&#160;      update_visualizer_ (output, *indices_, *target_, *indices_);</div>
<div class="line"><a name="l00475"></a><span class="lineno">  475</span>&#160; </div>
<div class="line"><a name="l00476"></a><span class="lineno">  476</span>&#160;    <span class="comment">//std::cout &lt;&lt; &quot;eps=&quot; &lt;&lt; fabs ((transformation - previous_transformation_).sum ()) &lt;&lt; std::endl;</span></div>
<div class="line"><a name="l00477"></a><span class="lineno">  477</span>&#160; </div>
<div class="line"><a name="l00478"></a><span class="lineno">  478</span>&#160;    <span class="keywordflow">if</span> (nr_iterations_ &gt; max_iterations_ ||</div>
<div class="line"><a name="l00479"></a><span class="lineno">  479</span>&#160;       (transformation - previous_transformation_).array ().abs ().sum () &lt; transformation_epsilon_)</div>
<div class="line"><a name="l00480"></a><span class="lineno">  480</span>&#160;      converged_ = <span class="keyword">true</span>;</div>
<div class="line"><a name="l00481"></a><span class="lineno">  481</span>&#160;  }</div>
<div class="line"><a name="l00482"></a><span class="lineno">  482</span>&#160;  final_transformation_ = transformation;</div>
<div class="line"><a name="l00483"></a><span class="lineno">  483</span>&#160;  output = intm_cloud;</div>
<div class="line"><a name="l00484"></a><span class="lineno">  484</span>&#160;}</div>
<div class="line"><a name="l00485"></a><span class="lineno">  485</span>&#160; </div>
<div class="line"><a name="l00486"></a><span class="lineno">  486</span>&#160;<span class="preprocessor">#endif    </span><span class="comment">// PCL_NDT_2D_IMPL_H_</span></div>
<div class="line"><a name="l00487"></a><span class="lineno">  487</span>&#160; </div>
<div class="ttc" id="aclasspcl_1_1_normal_distributions_transform2_d_html_abf99a15fe6808f4242da02091ec4b736"><div class="ttname"><a href="classpcl_1_1_normal_distributions_transform2_d.html#abf99a15fe6808f4242da02091ec4b736">pcl::NormalDistributionsTransform2D::computeTransformation</a></div><div class="ttdeci">virtual void computeTransformation(PointCloudSource &amp;output, const Eigen::Matrix4f &amp;guess)</div><div class="ttdoc">Rigid transformation computation method with initial guess.</div><div class="ttdef"><b>Definition:</b> ndt_2d.hpp:377</div></div>
<div class="ttc" id="aclasspcl_1_1_point_cloud_html"><div class="ttname"><a href="classpcl_1_1_point_cloud.html">pcl::PointCloud</a></div><div class="ttdoc">PointCloud represents the base class in PCL for storing collections of 3D points.</div><div class="ttdef"><b>Definition:</b> point_cloud.h:173</div></div>
<div class="ttc" id="aclasspcl_1_1ndt2d_1_1_n_d_t2_d_html"><div class="ttname"><a href="classpcl_1_1ndt2d_1_1_n_d_t2_d.html">pcl::ndt2d::NDT2D</a></div><div class="ttdoc">Build a Normal Distributions Transform of a 2D point cloud. This consists of the sum of four overlapp...</div><div class="ttdef"><b>Definition:</b> ndt_2d.hpp:306</div></div>
<div class="ttc" id="aclasspcl_1_1ndt2d_1_1_n_d_t2_d_html_ac85a7448e491a3e7bbba99f482af9375"><div class="ttname"><a href="classpcl_1_1ndt2d_1_1_n_d_t2_d.html#ac85a7448e491a3e7bbba99f482af9375">pcl::ndt2d::NDT2D::NDT2D</a></div><div class="ttdeci">NDT2D(PointCloudConstPtr cloud, const Eigen::Vector2f &amp;about, const Eigen::Vector2f &amp;extent, const Eigen::Vector2f &amp;step)</div><div class="ttdef"><b>Definition:</b> ndt_2d.hpp:318</div></div>
<div class="ttc" id="aclasspcl_1_1ndt2d_1_1_n_d_t2_d_html_ad8bef04c21a6db1db8dec5f758e0a87b"><div class="ttname"><a href="classpcl_1_1ndt2d_1_1_n_d_t2_d.html#ad8bef04c21a6db1db8dec5f758e0a87b">pcl::ndt2d::NDT2D::test</a></div><div class="ttdeci">ValueAndDerivatives&lt; 3, double &gt; test(const PointT &amp;transformed_pt, const double &amp;cos_theta, const double &amp;sin_theta) const</div><div class="ttdoc">Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distr...</div><div class="ttdef"><b>Definition:</b> ndt_2d.hpp:337</div></div>
<div class="ttc" id="aclasspcl_1_1ndt2d_1_1_n_d_t_single_grid_html"><div class="ttname"><a href="classpcl_1_1ndt2d_1_1_n_d_t_single_grid.html">pcl::ndt2d::NDTSingleGrid</a></div><div class="ttdoc">Build a set of normal distributions modelling a 2D point cloud, and provide the value and derivatives...</div><div class="ttdef"><b>Definition:</b> ndt_2d.hpp:219</div></div>
<div class="ttc" id="aclasspcl_1_1ndt2d_1_1_n_d_t_single_grid_html_a097042ef93cf30db0cc0002ad80312b5"><div class="ttname"><a href="classpcl_1_1ndt2d_1_1_n_d_t_single_grid.html#a097042ef93cf30db0cc0002ad80312b5">pcl::ndt2d::NDTSingleGrid::normalDistForPoint</a></div><div class="ttdeci">NormalDist * normalDistForPoint(PointT const &amp;p) const</div><div class="ttdoc">Return the normal distribution covering the location of point p</div><div class="ttdef"><b>Definition:</b> ndt_2d.hpp:275</div></div>
<div class="ttc" id="aclasspcl_1_1ndt2d_1_1_n_d_t_single_grid_html_af77c36218ee572918dcef6435e6650a8"><div class="ttname"><a href="classpcl_1_1ndt2d_1_1_n_d_t_single_grid.html#af77c36218ee572918dcef6435e6650a8">pcl::ndt2d::NDTSingleGrid::test</a></div><div class="ttdeci">ValueAndDerivatives&lt; 3, double &gt; test(const PointT &amp;transformed_pt, const double &amp;cos_theta, const double &amp;sin_theta) const</div><div class="ttdoc">Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distr...</div><div class="ttdef"><b>Definition:</b> ndt_2d.hpp:259</div></div>
<div class="ttc" id="aclasspcl_1_1ndt2d_1_1_normal_dist_html"><div class="ttname"><a href="classpcl_1_1ndt2d_1_1_normal_dist.html">pcl::ndt2d::NormalDist</a></div><div class="ttdoc">A normal distribution estimation class.</div><div class="ttdef"><b>Definition:</b> ndt_2d.hpp:97</div></div>
<div class="ttc" id="aclasspcl_1_1ndt2d_1_1_normal_dist_html_a35b40158d8a0c8884413d2219b3917ba"><div class="ttname"><a href="classpcl_1_1ndt2d_1_1_normal_dist.html#a35b40158d8a0c8884413d2219b3917ba">pcl::ndt2d::NormalDist::addIdx</a></div><div class="ttdeci">void addIdx(size_t i)</div><div class="ttdoc">Store a point index to use later for estimating distribution parameters.</div><div class="ttdef"><b>Definition:</b> ndt_2d.hpp:110</div></div>
<div class="ttc" id="aclasspcl_1_1ndt2d_1_1_normal_dist_html_ad72ee95c77c17324d683c34915374cf1"><div class="ttname"><a href="classpcl_1_1ndt2d_1_1_normal_dist.html#ad72ee95c77c17324d683c34915374cf1">pcl::ndt2d::NormalDist::estimateParams</a></div><div class="ttdeci">void estimateParams(const PointCloud &amp;cloud, double min_covar_eigvalue_mult=0.001)</div><div class="ttdoc">Estimate the normal distribution parameters given the point indices provided. Memory of point indices...</div><div class="ttdef"><b>Definition:</b> ndt_2d.hpp:120</div></div>
<div class="ttc" id="aclasspcl_1_1ndt2d_1_1_normal_dist_html_aebf7b0204e59622dbf5fce66edfb8f7c"><div class="ttname"><a href="classpcl_1_1ndt2d_1_1_normal_dist.html#aebf7b0204e59622dbf5fce66edfb8f7c">pcl::ndt2d::NormalDist::test</a></div><div class="ttdeci">ValueAndDerivatives&lt; 3, double &gt; test(const PointT &amp;transformed_pt, const double &amp;cos_theta, const double &amp;sin_theta) const</div><div class="ttdoc">Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distr...</div><div class="ttdef"><b>Definition:</b> ndt_2d.hpp:165</div></div>
<div class="ttc" id="agroup__common_html_ga52d532f7f2b4d7bba78d13701d3a33d8"><div class="ttname"><a href="group__common.html#ga52d532f7f2b4d7bba78d13701d3a33d8">pcl::transformPointCloud</a></div><div class="ttdeci">void transformPointCloud(const pcl::PointCloud&lt; PointT &gt; &amp;cloud_in, pcl::PointCloud&lt; PointT &gt; &amp;cloud_out, const Eigen::Transform&lt; Scalar, 3, Eigen::Affine &gt; &amp;transform, bool copy_all_fields=true)</div><div class="ttdoc">Apply an affine transform defined by an Eigen Transform</div><div class="ttdef"><b>Definition:</b> transforms.hpp:42</div></div>
<div class="ttc" id="astructpcl_1_1_point_x_y_z_r_g_b_a_html"><div class="ttname"><a href="structpcl_1_1_point_x_y_z_r_g_b_a.html">pcl::PointXYZRGBA</a></div><div class="ttdoc">A point structure representing Euclidean xyz coordinates, and the RGBA color.</div><div class="ttdef"><b>Definition:</b> point_types.hpp:540</div></div>
<div class="ttc" id="astructpcl_1_1ndt2d_1_1_value_and_derivatives_html"><div class="ttname"><a href="structpcl_1_1ndt2d_1_1_value_and_derivatives.html">pcl::ndt2d::ValueAndDerivatives</a></div><div class="ttdoc">Class to store vector value and first and second derivatives (grad vector and hessian matrix),...</div><div class="ttdef"><b>Definition:</b> ndt_2d.hpp:57</div></div>
</div><!-- fragment --></div><!-- contents -->
</div><!-- doc-content -->
<!-- start footer part -->
<div id="nav-path" class="navpath"><!-- id is needed for treeview function! -->
  <ul>
    <li class="navelem"><a class="el" href="dir_05a7d3fa63398329a063418701e459b8.html">registration</a></li><li class="navelem"><a class="el" href="dir_4ae07afb78675be4cbd22b293e4a6696.html">include</a></li><li class="navelem"><a class="el" href="dir_2fef8963f1cfbd0f6472a197f46a93e4.html">pcl</a></li><li class="navelem"><a class="el" href="dir_003a12cd6c43fd0e3458933fbdf52053.html">registration</a></li><li class="navelem"><a class="el" href="dir_6fe9a4b68e13565b33d382f6516d3b0b.html">impl</a></li><li class="navelem"><b>ndt_2d.hpp</b></li>
    <li class="footer">制作者 <a href="https://www.doxygen.org/index.html"><img class="footer" src="doxygen.svg" width="104" height="31" alt="doxygen"/></a> 1.9.1 </li>
  </ul>
</div>
</body>
</html>
